#include "multicam_fusion.h"

using namespace Eigen;
using namespace std;    

// output data
ros::Publisher estimate_pose_pub;
ros::Publisher estimate_vel_pub;
ros::Publisher estimate_pose_f_pub;
ros::Publisher estimate_vel_f_pub;
ros::Publisher estimate_viconpose_pub;
ros::Publisher estimate_viconvel_pub;
ros::Publisher estimate_orientation_pub;

//check rel
ros::Publisher estimate_relpose_pub;

ros::Publisher param_cam_pub;
ros::Publisher uesd_imu_pub;

ros::Publisher uesd_dt_pub;
ros::Publisher camera_A_pub;
ros::Publisher camera_B_pub;
ros::Publisher camera_C_pub;
ros::Publisher camera_D_pub;
ros::Publisher camera_ICP_pub;
ros::Publisher camera_TAG_pub;
ros::Publisher cameraA_a2g_pub;
ros::Publisher estimate_est_relpose_pub;
ros::Publisher estimate_est_relvel_pub;

// ================================ Function Declaration and Definition ========================= //
void PubPose(const MatrixXd& data, int num, ros::Publisher& publisher, int start_index) {
    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.stamp = ros::Time::now();
    pose_msg.pose.position.x = data(start_index, num);
    pose_msg.pose.position.y = data(start_index + 1, num);
    pose_msg.pose.position.z = data(start_index + 2, num);
    publisher.publish(pose_msg);
}

void PubPoseandOri(MatrixXd data,MatrixXd data_ori, int num, ros::Publisher& publisher, int start_index) {
    geometry_msgs::PoseStamped pose_msg;
    pose_msg.header.stamp = ros::Time::now();
    pose_msg.pose.position.x = data(start_index, num);
    pose_msg.pose.position.y = data(start_index + 1, num);
    pose_msg.pose.position.z = data(start_index + 2, num);

    pose_msg.pose.orientation.x = data_ori(start_index + 1, num);
    pose_msg.pose.orientation.y = data_ori(start_index + 2, num);
    pose_msg.pose.orientation.z = data_ori(start_index + 3, num);
    pose_msg.pose.orientation.w = data_ori(start_index, num);

    publisher.publish(pose_msg);
}

void PubVelocity(const MatrixXd& data, int num, ros::Publisher& publisher, int start_index) {
    geometry_msgs::TwistStamped vel_msg;
    vel_msg.header.stamp = ros::Time::now();
    vel_msg.twist.linear.x = data(start_index, num);
    vel_msg.twist.linear.y = data(start_index + 1, num);
    vel_msg.twist.linear.z = data(start_index + 2, num);
    publisher.publish(vel_msg);
}

// used data : MHE_dt
void Pubdt(float _used_dt,float _used_dt_solveonce ,dwe *filter){
    std_msgs::Float32MultiArray dt_msg;
    dt_msg.data.resize(2); // 设置数组大小为2

    // 填充数据
    dt_msg.data[0] = _used_dt;
    dt_msg.data[1] = _used_dt_solveonce;

    uesd_dt_pub.publish(dt_msg); 
}


void Pubparamcam(MatrixXd _cam_param, dwe *filter) {
    std_msgs::Float32MultiArray param_cam_msg;
    param_cam_msg.data.resize(3*(filter->camnum_)); // 设置数组大小为4

    // 填充数据
    for(int i=0; i<filter->camnum_;i++){
        param_cam_msg.data[3*i]   = _cam_param(3*i, filter->num_state_);
        param_cam_msg.data[3*i+1] = _cam_param(3*i+1, filter->num_state_);
        param_cam_msg.data[3*i+2] = _cam_param(3*i+2, filter->num_state_);
    }

    // 发布消息
    param_cam_pub.publish(param_cam_msg);
}

// ================================ Main Function ========================= //
int main (int argc, char** argv) 
{
    // ros init
    ros::init(argc, argv, "dwe", ros::init_options::AnonymousName);
    ros::NodeHandle nh;
    ros::Rate loop_rate(25);

    // pub the tf of estimation
    // tf::TransformBroadcaster* br = new(tf::TransformBroadcaster);

    // define dwe_filter
    dwe dwe_filter(nh);

    //==================== subscriber =====================//
    ros::Subscriber imu_new_sub = nh.subscribe<sensor_msgs::Imu>("/IMU_data",1,boost::bind(&dwe::imu_new_cb, &dwe_filter, _1));

    ros::Subscriber pos_uav_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vicon_uav",1,boost::bind(&dwe::pos_uav_cb, &dwe_filter, _1));
    ros::Subscriber vel_uav_sub = nh.subscribe<geometry_msgs::TwistStamped>("/uav201/mocap/vel", 1,boost::bind(&dwe::vel_uav_cb, &dwe_filter, _1));

    // get the data from camera
    ros::Subscriber camA_sub = nh.subscribe<geometry_msgs::TransformStamped>("/camA/single_cam_process_ros/ir_mono/T_base_to_estimation", 1,boost::bind(&dwe::camA_cb, &dwe_filter, _1));
    ros::Subscriber camB_sub = nh.subscribe<geometry_msgs::TransformStamped>("/camB/single_cam_process_ros/ir_mono/T_base_to_estimation", 1,boost::bind(&dwe::camB_cb, &dwe_filter, _1));
    ros::Subscriber camC_sub = nh.subscribe<geometry_msgs::TransformStamped>("/camC/single_cam_process_ros/ir_mono/T_base_to_estimation", 1,boost::bind(&dwe::camC_cb, &dwe_filter, _1));
    ros::Subscriber camD_sub = nh.subscribe<geometry_msgs::TransformStamped>("/camD/single_cam_process_ros/ir_mono/T_base_to_estimation", 1,boost::bind(&dwe::camD_cb, &dwe_filter, _1));
    // get the data from CAM_A_MONO
    ros::Subscriber camA_a2g_sub = nh.subscribe<geometry_msgs::TransformStamped>("/CAM_A/air2surface_node_ros/mono/pnp_estimation", 1,boost::bind(&dwe::camA_a2g_cb, &dwe_filter, _1));
    // get the data from TAG
    ros::Subscriber camTAG_sub = nh.subscribe<geometry_msgs::TransformStamped>("/average_point", 1,boost::bind(&dwe::camTAG_cb, &dwe_filter, _1));

    //==================== publisher =====================//
    // pub : check -- use for rosbag pla check
    estimate_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/xt_real/pose", 1);
    estimate_vel_pub = nh.advertise<geometry_msgs::TwistStamped>(dwe_filter.dataType_ + "/xt_real/vel", 1);

    estimate_pose_f_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/xt_real_f/pose", 1);
    estimate_vel_f_pub = nh.advertise<geometry_msgs::TwistStamped>(dwe_filter.dataType_ + "/xt_real_f/vel", 1);

    estimate_viconpose_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/viconxt_real/pose", 1);
    estimate_viconvel_pub = nh.advertise<geometry_msgs::TwistStamped>(dwe_filter.dataType_ + "/viconxt_real/vel", 1);

    estimate_est_relpose_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/xt_f/pose", 1);
    estimate_est_relvel_pub = nh.advertise<geometry_msgs::TwistStamped>(dwe_filter.dataType_ + "/xt_f/vel", 1);   

    estimate_relpose_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/viconxt/pose", 1);

    // pub : used data
    uesd_imu_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/used/imu", 1);       // with ori trans
    uesd_dt_pub = nh.advertise<std_msgs::Float32MultiArray>(dwe_filter.dataType_ + "/used/dt", 1);        // loop_dt & solveonce_dt

    // pub : just for data state check
    param_cam_pub = nh.advertise<std_msgs::Float32MultiArray>(dwe_filter.dataType_ + "/param/cam", 1);    // check the star of uwb1-4

    camera_A_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/cameraA/pose",1);
    camera_B_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/cameraB/pose",1);
    camera_C_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/cameraC/pose",1);
    camera_D_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/cameraD/pose",1);
    cameraA_a2g_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/cameraA_a2g/pose",1);
    camera_TAG_pub = nh.advertise<geometry_msgs::PoseStamped>(dwe_filter.dataType_ + "/cameraLIDAR/pose",1);
    // ================================ Main LOOP ========================= //
    while(ros::ok()){
 
        ros::spinOnce();

        // // ensure imu msg is obtained
        // if (!(dwe_filter.flag_init_imu_)){
        //     continue;
        // }

        // estimated initial value
        dwe_filter.update_data(); 

        // LOOP_num: current timestep
        ROS_INFO("----------ENTER ROS LOOP----------");
        ROS_INFO("LOOP: %d", dwe_filter.LOOP_num_);

        // show the data from uav
        ROS_INFO("DATA MHE_imu_: ux:%f uy:%f uz:%f",dwe_filter.MHE_imu_(0,dwe_filter.num_state_), 
            dwe_filter.MHE_imu_(1,dwe_filter.num_state_),dwe_filter.MHE_imu_(2,dwe_filter.num_state_));

        // ROS_INFO("DATA MHE_imu_att_: d1:%f d2:%f d3:%f d4:%f",
        //     dwe_filter.MHE_imu_att_(0,dwe_filter.num_state_), dwe_filter.MHE_imu_att_(1,dwe_filter.num_state_),
        //     dwe_filter.MHE_imu_att_(2,dwe_filter.num_state_), dwe_filter.MHE_imu_att_(3,dwe_filter.num_state_));

        // get initial data xt_ until takeoff
        if(!dwe_filter.flag_init_ok_){
            // xt_real_pos & xt_real_vel (origin estimation)
            PubPose(dwe_filter.xt_real_, dwe_filter.num_state_ , estimate_pose_pub, 0);
            PubVelocity(dwe_filter.xt_real_, dwe_filter.num_state_, estimate_vel_pub, 3);
            // xt_real_f_pos & xt_real_f_vel (filtered estimation)
            PubPoseandOri(dwe_filter.xt_real_f_,dwe_filter.MHE_imu_att_,dwe_filter.num_state_, estimate_pose_f_pub, 0);
            PubVelocity(dwe_filter.xt_real_f_,dwe_filter.num_state_, estimate_vel_f_pub, 3);

            // viconxt_real_pos & viconxt_real_vel
            PubPose(dwe_filter.VICON_PosUAV_, dwe_filter.num_state_, estimate_viconpose_pub, 0);
            PubVelocity(dwe_filter.VICON_VelUAV_, dwe_filter.num_state_, estimate_viconvel_pub, 0);
            // xt_pos & xt_vel (origin estimation)
            // used for ctrl relative
            // PubPoseandOri(dwe_filter.xt_,dwe_filter.MHE_imu_att_,dwe_filter.num_state_, estimate_est_relpose_pub, 0);
            // PubVelocity(dwe_filter.xt_real_f_,dwe_filter.num_state_, estimate_est_relvel_pub, 3);

            PubPoseandOri(dwe_filter.xt_f_,dwe_filter.MHE_imu_att_,dwe_filter.num_state_, estimate_est_relpose_pub, 0);
            PubVelocity(dwe_filter.xt_f_,dwe_filter.num_state_, estimate_est_relvel_pub, 3);

            // cam_pos
            PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_A_pub, 0);
            PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_B_pub, 3);
            PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_C_pub, 6);
            PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_D_pub, 9);
            PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, cameraA_a2g_pub, 12);
            // PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_TAG_pub, 15);
            // imu data
            PubPose(dwe_filter.MHE_imu_, dwe_filter.num_state_-1, uesd_imu_pub, 0);
            // PubPose(dwe_filter.VICON_ImuUAV_, dwe_filter.num_state_-1, uesd_imuvicon_pub, 0);
            // dt & dt_solveonce
            Pubdt(dwe_filter.dt_,0,&dwe_filter);
            Pubparamcam(dwe_filter.paramCAM_,&dwe_filter);
            dwe_filter.LOOP_num_ = dwe_filter.LOOP_num_+1;
            dwe_filter.flag_init_imu_ = false; 
            loop_rate.sleep();
            continue;
        }

        // estimated initial value
        double time_start = ros::Time::now().toSec();  
        dwe_filter.solveonce();  
        double time_end = ros::Time::now().toSec();
        ROS_INFO("consumptiom dt: %.4lf seconds", time_end - time_start);

        // show the Initialized data x1_ and the Estimated data xt_
        // ROS_INFO("Estimated   data: x:%f y:%f z:%f",dwe_filter.xt_real_(0,dwe_filter.num_state_), 
        //     dwe_filter.xt_real_(1,dwe_filter.num_state_),dwe_filter.xt_real_(2,dwe_filter.num_state_));
        // ROS_INFO("Error real  data: x:%f y:%f z:%f",
        //     (dwe_filter.xt_real_(0,dwe_filter.num_state_)-dwe_filter.VICON_PosUAV_(0,dwe_filter.num_state_)), 
        //     (dwe_filter.xt_real_(1,dwe_filter.num_state_)-dwe_filter.VICON_PosUAV_(1,dwe_filter.num_state_)),
        //     (dwe_filter.xt_real_(2,dwe_filter.num_state_)-dwe_filter.VICON_PosUAV_(2,dwe_filter.num_state_)));
       	
        // used for check when without vicon
        // ROS_INFO("Estimated   data: x:%f y:%f z:%f",dwe_filter.xt_real_f_(0,dwe_filter.num_state_), 
        //     dwe_filter.xt_real_f_(1,dwe_filter.num_state_),dwe_filter.xt_real_f_(2,dwe_filter.num_state_));
        ROS_INFO("Estimated   data: x:%f y:%f z:%f",dwe_filter.xt_f_(0,dwe_filter.num_state_), 
            dwe_filter.xt_f_(1,dwe_filter.num_state_),dwe_filter.xt_f_(2,dwe_filter.num_state_));
        ROS_WARN("Error real  data: x:%f y:%f z:%f",
            (dwe_filter.xt_f_(0,dwe_filter.num_state_)-dwe_filter.VICON_PosUAV_(0,0)), 
            (dwe_filter.xt_f_(1,dwe_filter.num_state_)-dwe_filter.VICON_PosUAV_(1,0)),
            (dwe_filter.xt_f_(2,dwe_filter.num_state_)-dwe_filter.VICON_PosUAV_(2,0)));
       	

        // 写入内容到文件
        // dwe_filter.data_writter();

        // xt_real_pos & xt_real_vel (origin estimation)
        PubPose(dwe_filter.xt_real_, dwe_filter.num_state_ , estimate_pose_pub, 0);
        PubVelocity(dwe_filter.xt_real_, dwe_filter.num_state_, estimate_vel_pub, 3);
        // xt_real_f_pos & xt_real_f_vel (filtered estimation)
        PubPoseandOri(dwe_filter.xt_real_f_,dwe_filter.MHE_imu_att_,dwe_filter.num_state_, estimate_pose_f_pub, 0);
        PubVelocity(dwe_filter.xt_real_f_,dwe_filter.num_state_, estimate_vel_f_pub, 3);

        // viconxt_real_pos & viconxt_real_vel
        PubPose(dwe_filter.VICON_PosUAV_, dwe_filter.num_state_, estimate_viconpose_pub, 0);
        PubVelocity(dwe_filter.VICON_VelUAV_, dwe_filter.num_state_, estimate_viconvel_pub, 0);
        // xt_pos & xt_vel (origin estimation)
        // used for ctrl relative
        // PubPoseandOri(dwe_filter.xt_,dwe_filter.MHE_imu_att_,dwe_filter.num_state_, estimate_est_relpose_pub, 0);
        // PubVelocity(dwe_filter.xt_real_f_,dwe_filter.num_state_, estimate_est_relvel_pub, 3);

        PubPoseandOri(dwe_filter.xt_f_,dwe_filter.MHE_imu_att_,dwe_filter.num_state_, estimate_est_relpose_pub, 0);
        PubVelocity(dwe_filter.xt_f_,dwe_filter.num_state_, estimate_est_relvel_pub, 3);

        // cam_pos
        PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_A_pub, 0);
        PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_B_pub, 3);
        PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_C_pub, 6);
        PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_D_pub, 9);
        PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, cameraA_a2g_pub, 12);
        // PubPose(dwe_filter.MHE_cam_, dwe_filter.num_state_, camera_TAG_pub, 15);

        // imu data
        PubPose(dwe_filter.MHE_imu_, dwe_filter.num_state_-1, uesd_imu_pub, 0);
        // PubPose(dwe_filter.VICON_ImuUAV_, dwe_filter.num_state_-1, uesd_imuvicon_pub, 0);
        // dt & dt_solveonce
        Pubdt(dwe_filter.dt_,time_end - time_start,&dwe_filter);
        Pubparamcam(dwe_filter.paramCAM_,&dwe_filter);
        // test-
        // if (dwe_filter.LOOP_num_ == 2){
        //     break;
        // }
        // test-

        dwe_filter.LOOP_num_ = dwe_filter.LOOP_num_+1;
        dwe_filter.flag_init_imu_ = false; 
        loop_rate.sleep();

    }
   
    ROS_INFO("----------FINISH ROS LOOP----------");
    return 0;
}
